#include "stm32f10x.h"                  // Device header
#include "serial.h"
#include "string.h"
#include "spider.h"
#include "delay.h"
//编码格式：UTF-8

extern unsigned char spider_runmode_sta;    //声明机器人运动控制标志位
extern unsigned short int spider_leg[6][3]; //声明外部总线舵机角度数组
extern float XYZ[3];//陀螺仪参数

unsigned char serial_printf_flag=1; //串口重定向控制

int fputc(int ch,FILE *p)  //串口重定向
{
	if(serial_printf_flag==1)//重定向至USART1
	{
		while((USART1->SR & 0x40)==RESET);
		USART1->DR=(unsigned char)ch;
		return ch;
	}
	if(serial_printf_flag==2)//重定向至USART2
	{
		while((USART2->SR & 0x40)==RESET);
		USART2->DR=(unsigned char)ch;
		return ch;
	}
	return ch;
}

int main(void)
{

	serial_init(115200);//串口初始化，配置USART1
	spider_init();      //机器人初始化

	while (1)
	{
		Delay_ms(500); //系统延迟,防止意外
		spider_run(spider_runmode_sta);		//机器人运行控制
		if(serial_getrxflag(2)==1)//如果接收到了数据包
		{

			if(strcmp(serial_2_RX_packet,"MODE_TZ")==0)//停止指令
			{
				spider_runmode_sta=0;
			}
			else if(strcmp(serial_2_RX_packet,"MODE_QJ")==0)//前进指令
			{
				spider_runmode_sta=1;
			}
			else if(strcmp(serial_2_RX_packet,"MODE_HT")==0)//后退指令
			{
				spider_runmode_sta=2;
			}
			else if(strcmp(serial_2_RX_packet,"MODE_ZZ")==0)//左转指令
			{
				spider_runmode_sta=3;
			}
			else if(strcmp(serial_2_RX_packet,"MODE_YZ")==0)//右转指令
			{
				spider_runmode_sta=4;
			}
			else if(strcmp(serial_2_RX_packet,"MODE_GS")==0)//跟随指令
			{
				spider_runmode_sta=5;
			}
			else if(strcmp(serial_2_RX_packet,"MODE_PH")==0)//平衡指令
			{
				spider_runmode_sta=6;
			}
			else if(strcmp(serial_2_RX_packet,"MODE_BZ")==0)//避障指令
			{
				spider_runmode_sta=7;
			}
		}
			Delay_us(100);//等待串口发送完毕,避免重定向冲突
			serial_printf_flag=2;//使用USART2作为重定向
			printf("********************************\r\n");//回车换行
			switch(spider_runmode_sta)//发送当前状态机模式
			{
				case 0:
					printf("MODE:stop\r\n");
					break;
				case 1:
					printf("MODE:GoForward\r\n");
					break;
				case 2:
					printf("MODE:Retreat\r\n");
					break;
				case 3:
					printf("MODE:LeftRotation\r\n");
					break;
				case 4:
					printf("MODE:RightRotation\r\n");
					break;
				case 5:
					printf("MODE:follow\r\n");
					break;
				case 6:
					printf("MODE:balance\r\n");
					break;
				case 7:
					printf("MODE:obstacle_avoidance\r\n");
					break;					
			}		
			printf("********************************\r\n");
			serial_printf_flag=1;//使用USART1作为重定向
	}
}


